/**
  **********************************************************************************
  * @file   : Track_Param.h
  * @brief  : 
  * @author ：Red_Edged_Dog
  * @date   ： December,2019
  * @version：1.0
  **********************************************************************************/
#ifndef   _TRACK_PARAM_
#define  _TRACK_PARAM_

#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include <iostream>
#include <math.h>

/* Private  ------------------------------------------------------------------*/
//value_define
#define pi      (float) 3.14159265358979
#define L_H     (float) 0.09
#define L_L     (float) 0.162
#define L_wholeLength   L_H+L_L
#define minH0   (float) 0.17  
#define maxH0   (float) 0.25
#define minS    (float)(sqrt(pow(L_wholeLength,2.0)-pow(maxH0,2.0))*2)
#define maxS    (float)(sqrt(pow(L_wholeLength,2.0)-pow(minH0,2.0))*2)
#define minSetH (float)0.0001
#define maxSetH (float)maxH0-(L_L-L_H)
#define minNum    0.005//防止浮点数尾数不精确 导致超出循环限制条件 无法执行最后一条语句
//function_define
#define myabs(x)  ((x)>=0?x:-x)
#define constrainValue(Value,maxV,minV)     {Value=(Value>=maxV)?maxV:(Value<=minV)?minV:Value;}

//public parameters for x,y&theta,gamma  公共参数
typedef struct
{
	float Track_T;//one period 一个周期时间
	float Track_Set_H;
	float Track_Set_S;
	float Track_H0;//distance between joint&ground   关节点和地面距离
	float L_HighPosition;//l1
	float L_LowPosition;//l2
	float L_Center;//L
}publicTrack_Param;

//Cartesian coordinate system 直角坐标系下的xy变量结构体
typedef struct 
{   
	float Track_xN;
	float Track_yN;
	float Track_xL;
	float Track_yL;
	publicTrack_Param pTrack_pam;
}xyTrack_Param;  //x_yTrack_Param


//theta,gamma变量结构体
typedef struct
{
	float gtTrack_thetaL;
	float gtTrack_gammaL;
	float gtTrack_thetaN;
	float gtTrack_gammaN;
	publicTrack_Param pTrack_pam;
}gtTrack_Param;//gamma_theta_Track_Param

typedef struct
{
  float	nowTime;
  float lastTime;
}Generator_Timer;

typedef struct
{
	float xRate;
	float yRate;
	float gammaRate;
	float thetaRate;
}Rate_t;

template <typename T>  void Doggo_Constrain(T &Value,T maxV,T minV);

void X_track(xyTrack_Param &xytrack_par, Generator_Timer &based_timer);
void Y_track(xyTrack_Param &xytrack_par, Generator_Timer &based_timer);

void get_theta(xyTrack_Param &xy_par, gtTrack_Param &gt_par);
void get_gamma(xyTrack_Param &xy_par,gtTrack_Param &gt_par);

void Generator_timerInit(Generator_Timer &based_timer);
publicTrack_Param   Pparam_Init(float setH0, float setH, float setS);
void Trackpam_Init(publicTrack_Param &p_par, xyTrack_Param &xy_par, gtTrack_Param &gt_par);
void L_Update(xyTrack_Param &xy_par);
Rate_t get_paramRate(xyTrack_Param &xy_par, gtTrack_Param &gt_par, Generator_Timer &baseTimer);
void Update_Param(xyTrack_Param &xyT_param, gtTrack_Param &gtT_param, Generator_Timer &based_timer);

	
	
#ifdef __cplusplus
}
#endif

#endif